#include <math.h>
#include "iic1.h"
#include "mpu6050.h"

#define MPU6050_DEVICE_ADDR 0x68
#define WHO_AM_I 0x75
// 判断芯片是否存在
static uint8_t existenceFlag = 0;

//IIC1写入外设某一个位
void i2cdevWriteBit(uint8_t memAddress, uint8_t bitNum, uint8_t data)
{
    uint8_t byte;
    i2c1Read(MPU6050_DEVICE_ADDR, memAddress, &byte, 1);
    byte = (data != 0) ? (byte | (1 << bitNum)) : (byte & ~(1 << bitNum));
    i2c1Write(MPU6050_DEVICE_ADDR, memAddress, &byte, 1);
}
//IIC1写入外设多个位
void i2cdevWriteBits(uint8_t memAddress, uint8_t bitStart, uint8_t length, uint8_t data)
{
    uint8_t byte;
    i2c1Read(MPU6050_DEVICE_ADDR, memAddress, &byte, 1);
    uint8_t mask = ((1 << length) - 1) << (bitStart - length + 1);
    data <<= (bitStart - length + 1); // shift data into correct position
    data &= mask;                     // zero all non-important bits in data
    byte &= ~(mask);                  // zero all important bits in existing byte
    byte |= data;                     // combine data with existing byte
    i2c1Write(MPU6050_DEVICE_ADDR, memAddress, &byte, 1);
}

uint8_t mpu6050Init()
{
    uint8_t id = 0;
	Ddl_Delay1ms(10);
    i2c1Read(MPU6050_DEVICE_ADDR, WHO_AM_I, &id, 1);
	Ddl_Delay1ms(10);
    if (id == 0x68)
    {
        existenceFlag = 1;
        // 复位 
		i2cdevWriteBit(MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_DEVICE_RESET_BIT, 1);
        Ddl_Delay1ms(50);
        // 唤醒
        i2cdevWriteBit(MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_SLEEP_BIT, 0);
        Ddl_Delay1ms(10);
        // 设置X轴陀螺作为时钟
        i2cdevWriteBits(MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CLKSEL_BIT, MPU6050_PWR1_CLKSEL_LENGTH, MPU6050_CLOCK_PLL_XGYRO);
        Ddl_Delay1ms(10);                               // 延时等待时钟稳定
        // 使能温度传感器
        i2cdevWriteBit(MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_TEMP_DIS_BIT, 0);
        // 关闭中断
        uint8_t byte = 0;
        i2c1Write(MPU6050_DEVICE_ADDR, MPU6050_RA_INT_ENABLE, &byte, 1);
                 
        // 设置陀螺量程 
        i2cdevWriteBits(MPU6050_RA_GYRO_CONFIG, MPU6050_GCONFIG_FS_SEL_BIT,MPU6050_GCONFIG_FS_SEL_LENGTH, MPU6050_GYRO_FS_2000);
        // 设置加速计量程
        i2cdevWriteBits(MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_AFS_SEL_BIT, MPU6050_ACONFIG_AFS_SEL_LENGTH, MPU6050_ACCEL_FS_16);
        // 设置加速计数字低通滤波
        i2cdevWriteBits(MPU6050_RA_ACCEL_CONFIG_2, MPU6050_ACONFIG2_DLPF_BIT, MPU6050_ACONFIG2_DLPF_LENGTH, MPU6050_ACCEL_DLPF_BW_41);

        // 设置采样速率: 1000 / (1 + 0) = 1000Hz
        byte = 0;
        i2c1Write(MPU6050_DEVICE_ADDR, MPU6050_RA_SMPLRT_DIV, &byte, 1);
        // 设置陀螺数字低通滤波
        i2cdevWriteBits(MPU6050_RA_CONFIG, MPU6050_CFG_DLPF_CFG_BIT, MPU6050_CFG_DLPF_CFG_LENGTH, MPU6050_DLPF_BW_98);
    }
    return existenceFlag;
}

void mpu6050Data(int16_t *acc, int16_t *gyro)
{
    if (existenceFlag)
	{
		uint8_t buffer[14];
        i2c1Read(MPU6050_DEVICE_ADDR, MPU6050_RA_ACCEL_XOUT_H, buffer, 14);
        int16_t t_sensorAcc[3];
		int16_t t_sensorGyro[3];
        t_sensorAcc[X] = (((int16_t) buffer[0]) << 8) | buffer[1];
        t_sensorAcc[Y] = ((((int16_t) buffer[2]) << 8) | buffer[3]);
        t_sensorAcc[Z] = (((int16_t) buffer[4]) << 8) | buffer[5];
        t_sensorGyro[X] = (((int16_t) buffer[8]) << 8) | buffer[9];
        t_sensorGyro[Y] = (((int16_t) buffer[10]) << 8) | buffer[11];
        t_sensorGyro[Z] = (((int16_t) buffer[12]) << 8) | buffer[13];
		
			
		acc[Y] = t_sensorAcc[X];
		acc[X] = t_sensorAcc[Y];
		acc[Z] = t_sensorAcc[Z];
		gyro[Y] = -t_sensorGyro[X];
		gyro[X] = -t_sensorGyro[Y];
		gyro[Z] = t_sensorGyro[Z];
    }
}
